cmake_minimum_required(VERSION 2.8.3)
project(caros_universalrobot)

# ######################################################################################################################
# Local Variables
# ######################################################################################################################
set(node_name "${PROJECT_NAME}_node")
set(library_name "${PROJECT_NAME}_lib")
set(SHOULD_BUILD_CAROS_UNIVERSALROBOT 1)

# ######################################################################################################################
# Make use of c++11
# ######################################################################################################################
# See: http://answers.ros.org/question/173947/export-gcc-flags-though-catkin-cfg_extras/ Should automatically use c++11
# due to using caros_control Using hardcoded path-strings instead of a variable substitution for the use-c++11.cmake
# path-name, as catkin_lint doesn't properly do variable substitution (at least in this particular situation)
include("cmake/use-c++11.cmake")

# ######################################################################################################################
# RobWork
# ######################################################################################################################
set(RW_ROOT "$ENV{RW_ROOT}")
find_package(RobWork PATHS "${RW_ROOT}")
if(NOT ROBWORK_FOUND)
    message(WARNING "caros_universalrobot requires RobWork, this package will not be built!")
    set(SHOULD_BUILD_CAROS_UNIVERSALROBOT 0)
endif()

# ######################################################################################################################
# RobWorkHardware
# ######################################################################################################################
set(RWHW_ROOT "$ENV{RWHW_ROOT}")
find_package(RobWorkHardware COMPONENTS universalrobots_rtde PATHS "${RWHW_ROOT}")
if(NOT ROBWORKHARDWARE_FOUND)
    message(WARNING "Requires the universalrobots_rtde component of RobWorkHardware!")
    set(SHOULD_BUILD_CAROS_UNIVERSALROBOT 0)
endif()

# ######################################################################################################################
# Build process
# ######################################################################################################################
if(SHOULD_BUILD_CAROS_UNIVERSALROBOT)
    # ##################################################################################################################
    # Catkin Packages
    # ##################################################################################################################
    find_package(
        catkin REQUIRED
        COMPONENTS
            # cmake-format: sortable
            actionlib
            actionlib_msgs
            caros_common
            caros_common_msgs
            caros_common_robwork
            caros_common_robwork_msgs
            caros_control
            caros_control_msgs
            geometry_msgs
            message_generation
            rosconsole
            roscpp
            roslint
            sensor_msgs
            std_msgs
    )

    # ##################################################################################################################
    # Roslint
    # ##################################################################################################################
    set(ROSLINT_CPP_OPTS "--filter=-legal/copyright,-runtime/references")
    roslint_cpp()

    # ##################################################################################################################
    # Declare ROS messages, services and actions ##
    # ##################################################################################################################
    add_service_files(
        FILES
        UrServiceEmpty.srv
        UrServiceForceModeStart.srv
        UrServiceForceModeStop.srv
        UrServiceForceModeUpdate.srv
        UrServiceMoveLin.srv
        UrServiceMovePtp.srv
        UrServiceMovePtpPath.srv
        UrServiceMoveVelQ.srv
        UrServiceMoveVelT.srv
        UrServicePayload.srv
        UrServiceServoQ.srv
        UrServiceServoUpdate.srv
        UrServiceSetIO.srv
    )

    add_action_files(
        DIRECTORY
        action
        FILES
        ForceModeStart.action
        ForceModeStop.action
        ForceModeUpdate.action
        MoveLin.action
        MovePtp.action
        MovePtpPath.action
        MoveServoQ.action
        MoveServoStop.action
        MoveStop.action
        MoveVelQ.action
        MoveVelStop.action
        MoveVelT.action
        SendCustomScriptFile.action
        SendCustomScriptFunction.action
        SetIO.action
        SetPayload.action
        ZeroFTsensor.action
    )

    generate_messages(
        DEPENDENCIES
        actionlib_msgs
        caros_common_msgs
        caros_common_robwork_msgs
        caros_control_msgs
        geometry_msgs
        sensor_msgs
        std_msgs
    )

    # ##################################################################################################################
    # catkin specific configuration ##
    # ##################################################################################################################
    catkin_package(
        INCLUDE_DIRS include
        LIBRARIES ${library_name}
        CATKIN_DEPENDS
            actionlib
            actionlib_msgs
            caros_common
            caros_common_msgs
            caros_common_robwork
            caros_common_robwork_msgs
            caros_control
            caros_control_msgs
            geometry_msgs
            message_runtime
            roscpp
            sensor_msgs
            std_msgs
        DEPENDS
            ROBWORK
            ROBWORKHARDWARE
            CFG_EXTRAS
            "use-c++11.cmake"
    )

    # ##################################################################################################################
    # Build
    # ##################################################################################################################
    include_directories(
        include
        ${ROBWORK_INCLUDE_DIR}
        ${ROBWORKHARDWARE_INCLUDE_DIRS}
        ${catkin_INCLUDE_DIRS}
    )

    add_library(
        ${library_name}
        src/ur_action_interface.cpp
        src/ur_ai_proxy.cpp
        src/ur_service_interface.cpp
        src/ur_si_proxy.cpp
    )

    add_dependencies(
        ${library_name}
        ${catkin_EXPORTED_TARGETS}
        caros_common
        caros_common_robwork
        caros_control
        ${caros_universalrobot_EXPORTED_TARGETS}
    )

    target_link_libraries(${library_name} ${catkin_LIBRARIES} ${ROBWORK_LIBRARIES})

    add_executable(
        ${node_name}
        src/universal_robots.cpp
        src/ur_main.cpp
        src/ur_service_interface.cpp
        src/ur_si_proxy.cpp
    )

    target_link_libraries(
        ${node_name}
        ${library_name}
        ${ROBWORK_LIBRARIES}
        ${ROBWORKHARDWARE_LIBRARIES}
        ${catkin_LIBRARIES}
    )

    add_dependencies(
        ${node_name}
        caros_common
        caros_common_robwork
        caros_control
        ${caros_universalrobot_EXPORTED_TARGETS}
    )

    # ##################################################################################################################
    # Testing
    # ##################################################################################################################
    if(CATKIN_ENABLE_TESTING)
        add_executable(simple_${PROJECT_NAME}_demo_using_move_ptp src/test/simple_demo_using_move_ptp.cpp)
        target_link_libraries(simple_${PROJECT_NAME}_demo_using_move_ptp ${catkin_LIBRARIES} ${library_name})

        add_executable(simple_${PROJECT_NAME}_demo_using_move_servo_q src/test/simple_demo_using_move_servo_q.cpp)
        target_link_libraries(simple_${PROJECT_NAME}_demo_using_move_servo_q ${catkin_LIBRARIES} ${library_name})

        add_executable(simple_${PROJECT_NAME}_demo_using_force_mode src/test/simple_demo_using_force_mode.cpp)
        target_link_libraries(simple_${PROJECT_NAME}_demo_using_force_mode ${catkin_LIBRARIES} ${library_name})

    endif()
else()
    message(WARNING "Skipping ${PROJECT_NAME} due to missing libraries - see earlier warnings.")
endif()
